//------------------------------------------------------------------------------
//  <copyright file="ImageOperations2DInterop.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#include "ImageOperations2DInterop.h"

using namespace System::Diagnostics;
using namespace System::IO;
using namespace System::Runtime::InteropServices;
using namespace System::Windows;
using namespace Microsoft::Robotics::Vision;

// This unnamed namespace wraps classes that are local to this translation unit.
namespace
{
    class VisionToolsImage
    {
    private:
        vt::CRGBImg* vti;

    public:
        /// <summary> 
        /// Creates an instance of the VisionToolsImage class
        /// </summary>
        VisionToolsImage(ImageFrameRgb^ image)
        {
            pin_ptr<RgbPix> pointerToArray = &(image->ImageData[0]);
            this->vti = new vt::CRGBImg((unsigned char*) pointerToArray, image->Width, image->Height, image->Stride);
        }

        /// <summary> 
        /// Destroys object
        /// </summary>
        ~VisionToolsImage()
        {
            delete this->vti;
        }

        /// <summary> 
        /// cast operator to VisionTools vt::CRGBImg
        /// </summary>
        operator vt::CRGBImg&()
        {
            return *(this->vti);
        }
    };
}

List< ImageEdgel^ >^ ImageOperations2DInterop::GetEdgels(ImageFrameRgb^ image)
{
    VisionToolsImage vtih(image);
    vt::vector<vt::EdgeSegment> pEdgels;
    vt::EdgeDetectParams edParams;
    HRESULT hr;

    // detect EdgeSegments
    hr = vt::VtCreateEdgeSegmentListUsingDoG(pEdgels, vtih, edParams);
    if (FAILED(hr))
    {
        throw gcnew Exception(String::Format("CreateEdgeSegment failed with result {0:X}", hr));
    }

    // convert unmanaged EdgeSegments to managed Edgels
    List<ImageEdgel ^>^ managedEdgels = gcnew List<ImageEdgel ^>();
    for (int i = 0; i < (int) pEdgels.size() ; ++i)
    {
        vt::EdgeSegment es = pEdgels[i];
        ImageEdgel ^ Edgel = gcnew ImageEdgel(&es);
        managedEdgels->Add(Edgel);
    }

    return managedEdgels;
}

List< List< ImageEdgel^ >^ >^ ImageOperations2DInterop::GetCurves(ImageFrameRgb^ image)
{
    VisionToolsImage vtih(image);
    vt::vector<vt::EdgeSegment> pEdgels;
    vt::vector<vt::vector<int> > pCurves;
    vt::EdgeDetectParams edParams;
    vt::LineDetectorParams ldParams;
    bool useDOG = true;
    HRESULT hr;

    // detect EdgeSegments
    hr = vt::VtCreateEdgeSegmentListUsingDoG(pEdgels, vtih, edParams);
    if (FAILED(hr))
    {
        throw gcnew Exception(String::Format("CreateEdgeSegment failed with result {0:X}", hr));
    }

    // Find curves from edgels
    hr = vt::VtFindCurveSegmentsFromEdges(pCurves, pEdgels, useDOG, image->Width, image->Height, ldParams);
    if (FAILED(hr))
    {
        throw gcnew Exception(String::Format("FindCurvesFromSegment failed with result {0:X}", hr));
    }

    // convert unmanaged CurveSegments to managed Curves
    List< List< ImageEdgel^ >^ >^ managedCurves = gcnew List< List< ImageEdgel^ >^ >();
    for (int i = 0; i < (int) pCurves.size() ; ++i)
    {
        vt::vector<int> curve = pCurves[i];
        List< ImageEdgel^ >^ curveManaged = gcnew List< ImageEdgel^ >();
        for (int j = 0; j < (int) curve.size() ; ++j)
        {
            vt::EdgeSegment es = pEdgels[ curve[i] ];
            ImageEdgel^ edgel = gcnew ImageEdgel(&es);
            curveManaged->Add( edgel );
        }
        managedCurves->Add(curveManaged);
    }

    return managedCurves;
}

List< ImageLine^ >^ ImageOperations2DInterop::GetLines(ImageFrameRgb^ image)
{
    VisionToolsImage vtih(image);
    vt::vector<vt::EdgeSegment> pEdgels;
    vt::vector<vt::vector<int> > pCurves;
    vt::vector<vt::LineSegment> pLines;
    vt::EdgeDetectParams edParams;
    vt::LineDetectorParams ldParams;
	ldParams.lineSimplificationThreshold = 12.0f;

	bool useDOG = true;
    HRESULT hr;

    // detect EdgeSegments
    hr = vt::VtCreateEdgeSegmentListUsingDoG(pEdgels, vtih, edParams);
    if (FAILED(hr))
    {
        throw gcnew Exception(String::Format("VtCreateEdgeSegmentList failed with result {0:X}", hr));
    }

    // Find curves from edgels
    hr = vt::VtFindCurveSegmentsFromEdges(pCurves, pEdgels, useDOG, image->Width, image->Height, ldParams);
    if (FAILED(hr))
    {
        throw gcnew Exception(String::Format("VtFindCurveSegments failed with result {0:X}", hr));
    }

    // Find lines from curves
    hr =  VtFindLinesFromCurves(pLines, pEdgels, pCurves, ldParams);
    if (FAILED(hr))
    {
        throw gcnew Exception(String::Format("VtFindLinesFromCurves failed with result {0:X}", hr));
    }

    // convert unmanaged Lines to managed Lines
    List< ImageLine^ >^ managedLines = gcnew List<ImageLine ^>();
    for (int i = 0; i < (int) pLines.size() ; ++i)
    {
        vt::LineSegment es = pLines[i];
        ImageLine ^ Line = gcnew ImageLine(&es);
        managedLines->Add(Line);
    }

    return managedLines;
}

List< ImageEllipse^ >^ ImageOperations2DInterop::GetEllipses(ImageFrameRgb^ image)
{
    VisionToolsImage vtih(image);
    vt::vector<vt::EdgeSegment> pEdgels;
    vt::vector<vt::vector<int> > pCurves;
    vt::vector<vt::EllipseSegment> pEllipses;
    vt::EdgeDetectParams edParams;
    vt::LineDetectorParams ldParams;
    vt::FitEllipseParams feParams;
    bool useDOG = true;
    HRESULT hr;

    // detect EdgeSegments
    hr = vt::VtCreateEdgeSegmentListUsingDoG(pEdgels, vtih, edParams);
    if (FAILED(hr))
    {
        throw gcnew Exception(String::Format("VtCreateEdgeSegmentList failed with result {0:X}", hr));
    }

    // Find curves from edgels
    hr = vt::VtFindCurveSegmentsFromEdges(pCurves, pEdgels, useDOG, image->Width, image->Height, ldParams);
    if (FAILED(hr))
    {
        throw gcnew Exception(String::Format("VtFindCurveSegments failed with result {0:X}", hr));
    }

    // Find ellipses from curves
    hr = vt::VtFitEllipsesToCurves(pEllipses, pCurves, pEdgels, feParams);
    if (FAILED(hr))
    {
        throw gcnew Exception(String::Format("VtFitEllipsesToCurves failed with result {0:X}", hr));
    }

    // convert unmanaged ellipses to managed ellipses
    List<ImageEllipse ^>^ managedEllipses = gcnew List<ImageEllipse ^>();
    for (int i = 0; i < (int) pEllipses.size() ; ++i)
    {
        vt::EllipseSegment es = pEllipses[i];
        ImageEllipse ^ ellipse = gcnew ImageEllipse(&es);
        managedEllipses->Add(ellipse);
    }

    return managedEllipses;
}

ImageFrameYuy2^ ImageOperations2DInterop::CreateImageFrameYuy2FromRawDataBuffer(int width, int height, IntPtr data, int lengthOfDataInBytes)
{
    ImageFrameYuy2^ image = gcnew ImageFrameYuy2(width, height);

    if (lengthOfDataInBytes != image->Width * image->Height * image->BytesPerPixel)
    {
        throw gcnew ArgumentException;
    }

    // copy data from incoming 'data' buffer to the internal image buffer
    pin_ptr<Yuy2Pix> q = &(image->ImageData[0]);
    {
        memcpy((void*)q, (const void*)data, (unsigned int)lengthOfDataInBytes);
    }

    return image;
}

ImageFrameGray8^ ImageOperations2DInterop::CreateImageFrameGray8FromBitmapFrame(BitmapFrame^ bitmap)
{
    if (bitmap->Format.BitsPerPixel != 8)
    {
        throw gcnew ArgumentException;
    }

    ImageFrameGray8^ image = gcnew ImageFrameGray8(bitmap->PixelWidth, bitmap->PixelHeight);
    pin_ptr<byte> q = &(image->ImageData[0]);
    int stride = image->Width;
    bitmap->CopyPixels(Int32Rect::Empty, (IntPtr)q, image->Width * image->Height * image->BytesPerPixel, stride);
    return image;
}

ImageFrameRgb^ ImageOperations2DInterop::LoadImageRgbFromFile(System::String^ filename)
{
    if (File::Exists(filename) == false)
    {
        throw  gcnew ArgumentException(String::Format("Cannot find file {0}", filename));
    }

    // load filename into VisionTools image
    vt::CRGBImg* vti = new vt::CRGBImg();
    IntPtr intPtr = Marshal::StringToHGlobalUni(filename);
    HRESULT hr = vt::VtLoadImage((wchar_t *)(void *)intPtr, *vti);
    if ( hr != S_OK )
    {
        throw  gcnew ArgumentException(String::Format("Unable to call VtLoadImage on file {0}", filename));
    }
    Marshal::FreeHGlobal(intPtr);

    // check image type
    int type = vti->GetType(); 
    if ( (EL_FORMAT(type) != EL_FORMAT_BYTE) || (PIX_FORMAT(type) != PIX_FORMAT_RGB) )
    {
        throw  gcnew ArgumentException(String::Format("File {0} does not contain a 24-bit RGB image", filename));
    }
    if (vti->StrideBytes() != vti->Width() * 3)
    {
        throw  gcnew ArgumentException(String::Format("File {0} contains an image with extra padding at the end of each row, {1} != {2}", filename, vti->StrideBytes(), vti->Width() * 3));
    }

    // copy VisionTools image to ImageFrame
    ImageFrameRgb^ image = gcnew ImageFrameRgb(vti->Width(), vti->Height());
    unsigned char* p = vti->BytePtr();
    pin_ptr<RgbPix> q = &(image->ImageData[0]);
    int npixels = vti->Width() * vti->Height();
    for (int i=0 ; i<npixels ; i++)
    {
        // VisionTools stores pixels in BGR order, so switch to RGB order
        q->Red = p[2];
        q->Green = p[1];
        q->Blue = p[0];
        p += 3;
        q++;
    }

    ////int lengthOfDataInBytes = vti->Width() * vti->Height() * 3;
    ////pin_ptr<RgbPix> q = &(image->ImageData[0]);
    ////{
    ////    memcpy((void*)q, (const void*)vti->BytePtr(), (unsigned int)lengthOfDataInBytes);
    ////}

    return image;
}

ImageFrameRgb^ ImageOperations2DInterop::ConvertYUY2ToRGBImage(ImageFrameYuy2^ yuy2Image)
{
    ImageFrameRgb^ rgbImage = gcnew ImageFrameRgb(yuy2Image->Width, yuy2Image->Height);
    int lengthInBytesYuy2 = yuy2Image->ImageData->Length * yuy2Image->BytesPerPixel;
    int lengthInBytesRgb = rgbImage->ImageData->Length * rgbImage->BytesPerPixel;
    pin_ptr<Yuy2Pix> yuy2ImagePtr = &(yuy2Image->ImageData[0]);
    pin_ptr<RgbPix> rgbImagePtr = &(rgbImage->ImageData[0]);

    // do the conversion
    ImageOperations2DInterop::YUY2ToRGB((byte*)yuy2ImagePtr, (unsigned int)lengthInBytesYuy2, (byte*)rgbImagePtr, (unsigned int)lengthInBytesRgb);

    return rgbImage;
}

ImageFrameGray8^ ImageOperations2DInterop::ConvertYUY2ToGray8Image(ImageFrameYuy2^ yuy2Image)
{
    ImageFrameGray8^ lumImage = gcnew ImageFrameGray8(yuy2Image->Width, yuy2Image->Height);
    int lengthInBytesYuy2 = yuy2Image->ImageData->Length * yuy2Image->BytesPerPixel;
    int lengthInBytesGray8 = lumImage->ImageData->Length * lumImage->BytesPerPixel;
    pin_ptr<Yuy2Pix> yuy2ImagePtr = &(yuy2Image->ImageData[0]);
    pin_ptr<byte> lumImagePtr = &(lumImage->ImageData[0]);

    // do the conversion
    ImageOperations2DInterop::YUY2ToLum((byte*)yuy2ImagePtr, (unsigned int)lengthInBytesYuy2, lumImagePtr, (unsigned int)lengthInBytesGray8);

    return lumImage;
}

ImageFrameGray8^ ImageOperations2DInterop::ConvertRGBToGray8Image(ImageFrameRgb^ rgbImage)
{
    ImageFrameGray8^ lumImage = gcnew ImageFrameGray8(rgbImage->Width, rgbImage->Height);
    int lengthInBytesRgb = rgbImage->ImageData->Length * rgbImage->BytesPerPixel;
    int lengthInBytesGray8 = lumImage->ImageData->Length * lumImage->BytesPerPixel;
    pin_ptr<RgbPix> rgbImagePtr = &(rgbImage->ImageData[0]);
    pin_ptr<byte> lumImagePtr = &(lumImage->ImageData[0]);

    // do the conversion
    ImageOperations2DInterop::RGBToLum((byte*)rgbImagePtr, (unsigned int)lengthInBytesRgb, lumImagePtr, (unsigned int)lengthInBytesGray8);

    return lumImage;
}

/// <summary>
/// YUY2 to Luminance Conversion
/// </summary>
///<param name="pYUY2">YUY2 byte array containing the image, input - caller allocates memory</param>
///<param name="cbLengthRGB">Length of the RGB image array in bytes</param>
///<param name="pLum">Luminance byte array containing the image, output - caller allocates memory</param>
///<param name="cbLengthLum">Length of the Luminance image array in bytes</param>
void ImageOperations2DInterop::YUY2ToLum(UINT8* pYUY2, DWORD cbLengthYUY2, UINT8* pLum, DWORD cbLengthLum)
{
    if (cbLengthYUY2 != cbLengthLum * 2)
    {
        throw gcnew System::ArgumentException("cbLengthYUY2 is not 2 times cbLengthLum");
    }

    // 2 pixels of YUY2 correspond to 1 pixel of Luminance
    for (DWORD i = 0; i < cbLengthLum; ++i)
    {
        pLum[i] = pYUY2[i << 1];
    }
}

/// <summary>
/// RBG to Luminance Conversion
/// http://msdn.microsoft.com/en-us/library/aa917087.aspx
/// </summary>
///<param name="pRGB">pRGB byte array containing the image, input - caller allocates memory</param>
///<param name="cbLengthRGB">Length of the RGB image array in bytes</param>
///<param name="pLum">Luminance byte array containing the image, output - caller allocates memory</param>
///<param name="cbLengthLum">Length of the Luminance image array in bytes</param>
void ImageOperations2DInterop::RGBToLum(UINT8* pRGB, DWORD cbLengthRGB, UINT8* pLum, DWORD cbLengthLum)
{
    if (cbLengthRGB != (double)cbLengthLum * 3)
    {
        throw gcnew System::ArgumentException("cbLengthRBG is not 3 times cbLengthLum");
    }

    for (DWORD j = 0; j < cbLengthRGB;)
    {
        int r = pRGB[j++];
        int g = pRGB[j++];
        int b = pRGB[j++];

        *pLum++ = Clamp(0, UCHAR_MAX, ((66 * r + 129 * g +  25 * b + 128) >> 8) +  16);
    }
}

/// <summary>
/// Clamp between high and low values
/// </summary>
///<param name="bLow">Low byte value which forms the lower limit for clamping</param>
///<param name="bHigh">High byte value which forms the upper limit for clamping</param>
///<param name="nVal">The value to be clamped</param>
///<returns>Clamped value</returns>
inline int ImageOperations2DInterop::Clamp(int bLow, int bHigh, int nVal)
{
    return nVal < bLow ? bLow : (nVal < bHigh) ? nVal : bHigh;
}

/// <summary>
/// YUY2 to RGB Conversion from MSDN
/// http://msdn.microsoft.com/en-us/library/aa917087.aspx
/// </summary>
///<param name="pYUY2">YUY2 byte array containing the image, input - caller allocates memory</param>
///<param name="cbLengthYUY2">Length of the YUY2 image array in bytes</param>
///<param name="pRGB">RGB byte array containing the image, output - caller allocates memory</param>
///<param name="cbLengthRGB">Length of the RGB image array in bytes</param>
void ImageOperations2DInterop::YUY2ToRGB(UINT8* pYUY, DWORD cbLengthYUY2, UINT8* pRGB, DWORD cbLengthRGB)
{    
    // Each pixel of YUY2 consists of 2 Ys and one U and one V
    // The chromas (U and V) get upsampled in RGB24.     
    // Hence 4 bytes of YUY2 correspond to 6 bytes of RGB
    // Y1(Common UV)  Y2 (Common UV)
    // R1G1B1           R2G2B2

    if (cbLengthRGB != (double)cbLengthYUY2 * 1.5)
    {
        throw gcnew ArgumentException("cbLengthRGB is not 1.5 times cbLengthYUY2");  
    }

    for (DWORD i = 0; i < cbLengthYUY2; i += 4)
    {
        int nC = 298 * (pYUY[i] - 16);
        int nD = pYUY[i + 1] - 128;	
        int nD1 = 516 * nD;
        int nE = pYUY[i + 3] - 128;
        int nE1 = 208 * nE;

        // Upsample the first luminance and the common chrominance.
        *pRGB++ = Clamp(0, UCHAR_MAX, (nC + (409 * nE) + 128) >> 8);
        *pRGB++ = Clamp(0, UCHAR_MAX, (nC - (100 * nD) - nE1 + 128) >> 8);
        *pRGB++ = Clamp(0, UCHAR_MAX, (nC + nD1 + 128) >> 8);

        // Upsample the second luminance and the common chrominance.
        nC = 298 * (pYUY[i + 2] - 16);
        *pRGB++ = Clamp(0, UCHAR_MAX, (nC + (409 * nE) + 128) >> 8);  
        *pRGB++ = Clamp(0, UCHAR_MAX, (nC - (100 * nD) - nE1 + 128) >> 8);
        *pRGB++ = Clamp(0, UCHAR_MAX, (nC + nD1 + 128) >> 8);      
    }
}

/// <summary>
/// Finds feature points in a gray scale image
/// </summary>
/// <param name="image">The grayscale image</param>
/// <param name="width">Width of the image</param>
/// <param name="height">Height of the image</param>
/// <param name="stride">Stride, in bytes, of a row</param>
/// <param name="maxFeatures">Maximum number of features</param>
/// <param name="x">Array of x coordinates of feature points</param>
/// <param name="y">Array of y coordinates of feature points</param>
/// <param name="detector">The type of feature to detect, use values in SunflowerConstants.cs</param>
/// <param name="culling">The type of culling to use, use values in SunflowerConstants.cs</param>
/// <returns>The actual number of features found</returns>
int ImageOperations2DInterop::FindFeaturePoints(BYTE *image, int width, int height, int stride, int maxFeatures, float *x, float *y, int detector, int culling){
	// Convert to sunflower data types
	SF_BYTE_IMAGE byteImage = SF_BYTE_IMAGE(width, height, stride, image);
	
	// Sunflower features
	ISimpleMatcher *pMatcher;
	
	// Create the simple matcher as a Sunflower feature matcher
	try
	{
		pMatcher = CreateSunflowerSimpleMatcher(NULL);
	}
	catch(Exception ^e)
	{
		throw gcnew Exception(e->Message);;
	}

	// Define parameters of the feature detector
	SunflowerSimpleMatcherParameters params;
    pMatcher->GetParameters(&params);
	params.detector_type = (DetectorType) detector;
    params.target_feature_count = maxFeatures;
    params.ratio_test_ratio = 0.0f;
	params.culling_type = (CullingType) culling;
    pMatcher->SetParameters(&params);

	// Find feature points and descriptors in an image
	pMatcher->AddImage(byteImage);

	// Get the number of features
	int count = pMatcher->GetFeatureCount(0);
	
	// returns the detected features in two floating point arrays
	const FeaturePoint* features = pMatcher->GetFeatures(0);
	
	for(int i = 0; i < maxFeatures && i < count; i++){
		x[i] = features[i].x;
		y[i] = features[i].y;
	}

	pMatcher->Release();
	return count;
}

